
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_bmi088.c
  * @author     baiyang
  * @date       2022-6-17
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_bmi088.h"

#include <common/time/gp_time.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
/*
  device registers, names follow datasheet conventions, with REGA_
  prefix for accel, and REGG_ prefix for gyro
 */
#define REGA_CHIPID        0x00
#define REGA_ERR_REG       0x02
#define REGA_STATUS        0x03
#define REGA_X_LSB         0x12
#define REGA_INT_STATUS_1  0x1D
#define REGA_TEMP_MSB      0x22
#define REGA_TEMP_LSB      0x23
#define REGA_CONF          0x40
#define REGA_RANGE         0x41
#define REGA_PWR_CONF      0x7C
#define REGA_PWR_CTRL      0x7D
#define REGA_SOFTRESET     0x7E
#define REGA_FIFO_CONFIG0  0x48
#define REGA_FIFO_CONFIG1  0x49
#define REGA_FIFO_DOWNS    0x45
#define REGA_FIFO_DATA     0x26
#define REGA_FIFO_LEN0     0x24
#define REGA_FIFO_LEN1     0x25

#define REGG_CHIPID        0x00
#define REGA_RATE_X_LSB    0x02
#define REGG_INT_STATUS_1  0x0A
#define REGG_INT_STATUS_2  0x0B
#define REGG_INT_STATUS_3  0x0C
#define REGG_FIFO_STATUS   0x0E
#define REGG_RANGE         0x0F
#define REGG_BW            0x10
#define REGG_LPM1          0x11
#define REGG_RATE_HBW      0x13
#define REGG_BGW_SOFTRESET 0x14
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA     0x3F

#define ACCEL_BACKEND_SAMPLE_RATE   1600
#define GYRO_BACKEND_SAMPLE_RATE    2000

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_bmi088_start(sensor_imu_backend *backend);
static bool sensor_imu_bmi088_update(sensor_imu_backend *backend);
static bool read_accel_registers(sensor_imu_bmi088_t bmi088,uint8_t reg, uint8_t *data, uint8_t len);
static bool write_accel_register(sensor_imu_bmi088_t bmi088, uint8_t reg, uint8_t v);
static bool setup_accel_config(sensor_imu_bmi088_t bmi088);
static bool accel_init(sensor_imu_bmi088_t bmi088);
static bool gyro_init(sensor_imu_bmi088_t bmi088);
static bool init(sensor_imu_bmi088_t bmi088);
static void read_fifo_accel(void *parameter);
static void read_fifo_gyro(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_imu_backend_ops bmi088_ops = {.sensor_imu_backend_destructor = NULL,
                                                   .update = sensor_imu_bmi088_update,
                                                   .accumulate = NULL,
                                                   .start = sensor_imu_bmi088_start,
                                                   .get_output_banner = NULL};

static const struct {
    uint8_t reg;
    uint8_t value;
} accel_config[] = {
    { REGA_CONF, 0xAC },
    // setup 24g range (16g for BMI085)
    { REGA_RANGE, 0x03 },
    // disable low-power mode
    { REGA_PWR_CONF, 0 },
    { REGA_PWR_CTRL, 0x04 },
    // setup FIFO for streaming X,Y,Z
    { REGA_FIFO_CONFIG0, 0x02 },
    { REGA_FIFO_CONFIG1, 0x50 },
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_bmi088_ctor(sensor_imu_bmi088_t bmi088,
                                    gp_device_t _dev_accel,
                                    gp_device_t _dev_gyro,
                                    enum RotationEnum _rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(bmi088, 0, sizeof(struct sensor_imu_bmi088));

    sensor_imu_backend_ctor(&bmi088->backend, &bmi088_ops, "bmi088");

    bmi088->rotation = _rotation;
    
    bmi088->dev_accel = _dev_accel;
    bmi088->dev_gyro = _dev_gyro;
}

sensor_imu_backend * sensor_imu_bmi088_probe(gp_device_t dev_accel,
                                gp_device_t dev_gyro,
                                enum RotationEnum rotation)
{
    if (!dev_accel || !dev_gyro) {
        return NULL;
    }

    sensor_imu_bmi088_t sensor = (sensor_imu_bmi088_t)rt_malloc(sizeof(struct sensor_imu_bmi088));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_bmi088_ctor(sensor, dev_accel, dev_gyro, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static void sensor_imu_bmi088_start(sensor_imu_backend *backend)
{
    sensor_imu_bmi088_t bmi088 = (sensor_imu_bmi088_t)backend;

    if (!sensor_imu_register_accel(&bmi088->accel_instance, ACCEL_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi088->dev_accel, bmi088->_accel_devtype)) ||
        !sensor_imu_register_gyro(&bmi088->gyro_instance, GYRO_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi088->dev_gyro, DEVTYPE_INS_BMI088))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(bmi088->gyro_instance, bmi088->rotation);
    sensor_imu_backend_set_accel_orientation(bmi088->accel_instance, bmi088->rotation);
    
    // setup callbacks
    devmgr_register_periodic_callback(bmi088->dev_accel, 1000000UL / ACCEL_BACKEND_SAMPLE_RATE, read_fifo_accel, bmi088);
    devmgr_register_periodic_callback(bmi088->dev_gyro, 1000000UL / GYRO_BACKEND_SAMPLE_RATE, read_fifo_gyro, bmi088);
}

static bool sensor_imu_bmi088_update(sensor_imu_backend *backend)
{
    sensor_imu_bmi088_t bmi088 = (sensor_imu_bmi088_t)backend;

    sensor_imu_backend_update_accel(backend, bmi088->accel_instance);
    sensor_imu_backend_update_gyro(backend, bmi088->gyro_instance);
    return true;
}

/*
  read from accelerometer registers, special SPI handling needed
*/
static bool read_accel_registers(sensor_imu_bmi088_t bmi088, uint8_t reg, uint8_t *data, uint8_t len)
{
    // when on I2C we just read normally
    if (devmgr_get_bus_type(bmi088->dev_accel) != BUS_TYPE_SPI) {
        return devmgr_read_registers(bmi088->dev_accel, reg, data, len);
    }
    // for SPI we need to discard the first returned byte. See
    // datasheet for explanation
    uint8_t b[len+2];
    b[0] = reg | 0x80;
    rt_memset(&b[1], 0, len+1);
    if (!devmgr_transfer(bmi088->dev_accel, b, len+2, b, len+2)) {
        return false;
    }
    rt_memcpy(data, &b[2], len);
    return true;
}

/*
  write to accel registers with retries. The SPI sensor may take
  several tries to correctly write a register
*/
static bool write_accel_register(sensor_imu_bmi088_t bmi088, uint8_t reg, uint8_t v)
{
    for (uint8_t i=0; i<8; i++) {
        devmgr_write_register(bmi088->dev_accel, reg, v, false);
        uint8_t v2 = 0;
        if (read_accel_registers(bmi088, reg, &v2, 1) && v2 == v) {
            return true;
        }
    }
    return false;
}

static bool setup_accel_config(sensor_imu_bmi088_t bmi088)
{
    if (bmi088->done_accel_config) {
        return true;
    }
    bmi088->accel_config_count++;
    for (uint8_t i=0; i<ARRAY_SIZE(accel_config); i++) {
        uint8_t v;
        if (!read_accel_registers(bmi088, accel_config[i].reg, &v, 1)) {
            return false;
        }
        if (v == accel_config[i].value) {
            continue;
        }
        if (!write_accel_register(bmi088, accel_config[i].reg, accel_config[i].value)) {
            return false;
        }
    }
    bmi088->done_accel_config = true;
    console_printf("BMI088: accel config OK (%u tries)\n", (unsigned)bmi088->accel_config_count);
    return true;
}

/*
  probe and initialise accelerometer
 */
static bool accel_init(sensor_imu_bmi088_t bmi088)
{
    devmgr_take_bus(bmi088->dev_accel);

    uint8_t v;

    // dummy ready on accel ChipID to init accel (see section 3 of datasheet)
    read_accel_registers(bmi088, REGA_CHIPID, &v, 1);

    if (!read_accel_registers(bmi088, REGA_CHIPID, &v, 1)) {
        goto failed;
    }

    switch (v) {
        case 0x1E:
            bmi088->_accel_devtype = DEVTYPE_INS_BMI088;
            bmi088->accel_range    = 24.0f;
            console_printf("BMI088: Found device\n");
            break;
        case 0x1F:
            bmi088->_accel_devtype = DEVTYPE_INS_BMI085;
            bmi088->accel_range    = 16.0f;
            console_printf("BMI085: Found device\n");
            break;
        default:
            goto failed;
    }

    if (!setup_accel_config(bmi088)) {
        console_printf("BMI08x: delaying accel config\n");
    }

    console_printf("BMI08x: found accel\n");

    devmgr_release_bus(bmi088->dev_accel);

    return true;

failed:
    devmgr_release_bus(bmi088->dev_accel);
    return false;
}

/*
  probe and initialise gyro
 */
static bool gyro_init(sensor_imu_bmi088_t bmi088)
{
    devmgr_take_bus(bmi088->dev_gyro);

    uint8_t v;
    if (!devmgr_read_registers(bmi088->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F) {
        goto failed;
    }

    /* Soft-reset gyro
        Return value of 'write_register()' is not checked.
        This commands has the tendency to fail upon soft-reset.
    */
    devmgr_write_register(bmi088->dev_gyro, REGG_BGW_SOFTRESET, 0xB6, false);
    rt_thread_mdelay(30);

    devmgr_setup_checked_registers(bmi088->dev_gyro, 5, 20);
    
    // setup 2000dps range
    if (!devmgr_write_register(bmi088->dev_gyro, REGG_RANGE, 0x00, true)) {
        goto failed;
    }

    // setup filter bandwidth 230Hz, no decimation
    if (!devmgr_write_register(bmi088->dev_gyro, REGG_BW, 0x81, true)) {
        goto failed;
    }

    // disable low-power mode
    if (!devmgr_write_register(bmi088->dev_gyro, REGG_LPM1, 0, true)) {
        goto failed;
    }

    // setup for filtered data
    if (!devmgr_write_register(bmi088->dev_gyro, REGG_RATE_HBW, 0x00, true)) {
        goto failed;
    }

    // setup FIFO for streaming X,Y,Z, with stop-at-full
    if (!devmgr_write_register(bmi088->dev_gyro, REGG_FIFO_CONFIG_1, 0x40, true)) {
        goto failed;
    }

    console_printf("BMI088: found gyro\n");

    return true;
failed:
    devmgr_release_bus(bmi088->dev_gyro);
    return false;
}

static bool init(sensor_imu_bmi088_t bmi088)
{
    devmgr_set_read_flag(bmi088->dev_accel, 0x80);
    devmgr_set_read_flag(bmi088->dev_gyro, 0x80);

    return accel_init(bmi088) && gyro_init(bmi088);
}

/*
  read accel fifo
 */
static void read_fifo_accel(void *parameter)
{
    sensor_imu_bmi088_t bmi088 = (sensor_imu_bmi088_t)parameter;

    if (!setup_accel_config(bmi088)) {
        return;
    }
    uint8_t len[2];
    if (!read_accel_registers(bmi088, REGA_FIFO_LEN0, len, 2)) {
        sensor_imu_backend_inc_accel_error_count(bmi088->accel_instance);
        return;
    }
    uint16_t fifo_length = len[0] + (len[1]<<8);
    if (fifo_length & 0x8000) {
        // empty
        return;
    }

    // don't read more than 8 frames at a time
    if (fifo_length > 8*7) {
        fifo_length = 8*7;
    }
    if (fifo_length == 0) {
        return;
    }
    
    uint8_t data[fifo_length];
    if (!read_accel_registers(bmi088, REGA_FIFO_DATA, data, fifo_length)) {
        sensor_imu_backend_inc_accel_error_count(bmi088->accel_instance);
        return;
    }
    // assume configured for 24g range
    const float scale = (1.0f/32768.0f) * GRAVITY_MSS * bmi088->accel_range;
    const uint8_t *p = &data[0];
    while (fifo_length >= 7) {
        /*
          the fifo frames are variable length, with the frame type in the first byte
         */
        uint8_t frame_len = 2;
        switch (p[0] & 0xFC) {
        case 0x84: {
            // accel frame
            frame_len = 7;
            const uint8_t *d = p+1;
            int16_t xyz[3] = {
                (int16_t)((uint16_t)(d[0] | (d[1]<<8))),
                (int16_t)((uint16_t)(d[2] | (d[3]<<8))),
                (int16_t)((uint16_t)(d[4] | (d[5]<<8)))};
            Vector3f_t accel = {xyz[0], xyz[1], xyz[2]};

            vec3_mult(&accel, &accel, scale);

            sensor_imu_backend_rotate_and_correct_accel(&bmi088->backend, bmi088->accel_instance, &accel);
            sensor_imu_backend_notify_new_accel_raw_sample(&bmi088->backend, bmi088->accel_instance, &accel, 0, false);
            break;
        }
        case 0x40:
            // skip frame
            frame_len = 2;
            break;
        case 0x44:
            // sensortime frame
            frame_len = 4;
            break;
        case 0x48:
            // fifo config frame
            frame_len = 2;
            break;
        case 0x50:
            // sample drop frame
            frame_len = 2;
            break;
        }
        p += frame_len;
        fifo_length -= frame_len;
    }

    if (bmi088->temperature_counter++ == 100) {
        bmi088->temperature_counter = 0;
        uint8_t tbuf[2];
        if (!read_accel_registers(bmi088, REGA_TEMP_MSB, tbuf, 2)) {
            sensor_imu_backend_inc_accel_error_count(bmi088->accel_instance);
        } else {
            uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5);
            int16_t temp_int11 = temp_uint11>1023?temp_uint11-2048:temp_uint11;
            float temp_degc = temp_int11 * 0.125f + 23;
            sensor_imu_backend_publish_temperature(&bmi088->backend, bmi088->accel_instance, temp_degc);
        }
    }
}

/*
  read gyro fifo
 */
static void read_fifo_gyro(void *parameter)
{
    sensor_imu_bmi088_t bmi088 = (sensor_imu_bmi088_t)parameter;

    uint8_t num_frames;
    if (!devmgr_read_registers(bmi088->dev_gyro, REGG_FIFO_STATUS, &num_frames, 1)) {
        sensor_imu_backend_inc_gyro_error_count(bmi088->gyro_instance);
        return;
    }
    const float scale = radians(2000.0f) / 32767.0f;
    const uint8_t max_frames = 8;

    struct {
        int16_t x;
        int16_t y;
        int16_t z;
    } data[max_frames];

    if (num_frames & 0x80) {
        // fifo overrun, reset, likely caused by scheduling error
        devmgr_write_register(bmi088->dev_gyro, REGG_FIFO_CONFIG_1, 0x40, true);
        goto check_next;
    }

    num_frames &= 0x7F;
    
    // don't read more than 8 frames at a time
    num_frames = MIN(num_frames, max_frames);
    if (num_frames == 0) {
        goto check_next;
    }

    if (!devmgr_read_registers(bmi088->dev_gyro, REGG_FIFO_DATA, (uint8_t *)data, num_frames*6)) {
        sensor_imu_backend_inc_gyro_error_count(bmi088->gyro_instance);
        goto check_next;
    }

    // data is 16 bits with 2000dps range
    for (uint8_t i = 0; i < num_frames; i++) {
        if (data[i].x == (int16_t)(0xffff)
            && data[i].y == (int16_t)(0xffff)
            && data[i].z == (int16_t)(0xffff)) {
            continue;
        }

        Vector3f_t gyro = {data[i].x, data[i].y, data[i].z};
        vec3_mult(&gyro, &gyro, scale);

        sensor_imu_backend_rotate_and_correct_gyro(&bmi088->backend, bmi088->gyro_instance, &gyro);
        sensor_imu_backend_notify_new_gyro_raw_sample(&bmi088->backend, bmi088->gyro_instance, &gyro, 0);
    }

check_next:
    {
        struct checkreg reg;
        if (!devmgr_check_next_register2(bmi088->dev_gyro, &reg)) {
            //log_register_change(dev_gyro->get_bus_id(), reg);
            sensor_imu_backend_inc_gyro_error_count(bmi088->gyro_instance);
        }
    }
}

/*------------------------------------test------------------------------------*/


